#include "distributedMapping.h"
#include <ros/ros.h>
#include <omp.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>

struct PointXYZIRPYT
{
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY; // preferred way of adding a XYZ+padding
    float roll;
    float pitch;
    float yaw;
    double time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
} EIGEN_ALIGN16;                    // enforce SSE padding for correct memory alignment

POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRPYT,
                                  (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, roll, roll)(float, pitch, pitch)(float, yaw, yaw)(double, time, time))

typedef PointXYZIRPYT PointTypePose;
typedef pcl::PointXYZI PointType;

ros::Time timeLaserInfoStamp;
/***dcl-slam define***/
string name;
int number_of_cores = 4;
pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtree_surrounding_keyposes(new pcl::KdTreeFLANN<pcl::PointXYZI>());
/***dcl-slam define***/

// 相关参数
string map_file_path, lid_topic_qy;
double lidar_end_time = 0, lidar_start_time = 0, lidar_mean_scantime = 0;
int scan_count_front = 0, odom_count_front = 0;
double last_timestamp_lidar_front = 0, last_timestamp_odom_front = 0;

deque<pcl::PointCloud<pcl::PointXYZI>::Ptr> lidar_buffer_front;
deque<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> lidar_buffer_color;
deque<nav_msgs::Odometry::ConstPtr> odom_buffer_front;
deque<nav_msgs::Odometry::ConstPtr> odom_buffer_for_map;
deque<double> lidar_time_buffer_front;
deque<double> odom_time_buffer_front;
// 是否发布稠密地图
bool flg_pub_mapHigh = false;

mutex mtx_buffer_front;

// 拼接地图的变换
PointTypePose trans2PointTypePose(float transformIn[])
{
    PointTypePose thisPose6D;
    thisPose6D.x = transformIn[3];
    thisPose6D.y = transformIn[4];
    thisPose6D.z = transformIn[5];
    thisPose6D.roll = transformIn[0];
    thisPose6D.pitch = transformIn[1];
    thisPose6D.yaw = transformIn[2];
    return thisPose6D;
}

pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose *transformIn)
{
    pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());

    int cloudSize = cloudIn->size();
    cloudOut->resize(cloudSize);

    Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw);

#pragma omp parallel for num_threads(numberOfCores)
    for (int i = 0; i < cloudSize; ++i)
    {
        const auto &pointFrom = cloudIn->points[i];
        cloudOut->points[i].x = transCur(0, 0) * pointFrom.x + transCur(0, 1) * pointFrom.y + transCur(0, 2) * pointFrom.z + transCur(0, 3);
        cloudOut->points[i].y = transCur(1, 0) * pointFrom.x + transCur(1, 1) * pointFrom.y + transCur(1, 2) * pointFrom.z + transCur(1, 3);
        cloudOut->points[i].z = transCur(2, 0) * pointFrom.x + transCur(2, 1) * pointFrom.y + transCur(2, 2) * pointFrom.z + transCur(2, 3);
        cloudOut->points[i].intensity = pointFrom.intensity;
    }
    return cloudOut;
}

// 雷达回调函数
void standard_pcl_vel(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
    // ROS_INFO("lidar 回调函数启动");
    mtx_buffer_front.lock();
    scan_count_front++;
    if (msg->header.stamp.toSec() < last_timestamp_lidar_front)
    {
        ROS_ERROR("lidar loop back, clear buffer");
        lidar_buffer_front.clear();
    }

    pcl::PointCloud<pcl::PointXYZI>::Ptr ptr(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr ptr_color(new pcl::PointCloud<pcl::PointXYZRGB>());
    // p_pre->process(msg, ptr);
    // 将msg转换为pcl
    pcl::fromROSMsg(*msg, *ptr);
    lidar_buffer_front.push_back(ptr);
    // pcl::fromROSMsg(*msg, *ptr_color);
    // lidar_buffer_color.push_back(ptr_color);
    lidar_time_buffer_front.push_back(msg->header.stamp.toSec());
    last_timestamp_lidar_front = msg->header.stamp.toSec();
    mtx_buffer_front.unlock();
    // sig_buffer.notify_all();
}

// 里程计轨迹回调函数
void odom_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
    // ROS_INFO("odom 回调函数启动");
    mtx_buffer_front.lock();
    odom_count_front++;
    if (lidar_buffer_front.empty())
    {
        mtx_buffer_front.unlock();
        return;
    }
    if (msg->header.stamp.toSec() < last_timestamp_odom_front)
    {
        ROS_ERROR("odom loop back, clear buffer");
        odom_buffer_front.clear();
    }

    odom_buffer_front.push_back(msg);
    odom_buffer_for_map.push_back(msg);
    last_timestamp_odom_front = msg->header.stamp.toSec();
    odom_time_buffer_front.push_back(msg->header.stamp.toSec());
    mtx_buffer_front.unlock();
}

using namespace std;
int main(int argc, char **argv)
{
    ros::init(argc, argv, "dclFront");
    ros::NodeHandle nh;
    setlocale(LC_ALL, "");
    distributedMapping dm;
    // 读取机器人的名称
    std::string ns = nh.getNamespace(); // namespace of robot
    if (ns.length() != 2)
    {
        ROS_ERROR("Invalid robot prefix (should be either 'a-z' or 'A-Z'): %s", ns.c_str());
        ros::shutdown();
    }
    name = ns.substr(1, 1); // leave '/'
    string name_ = ns.c_str();
    // ROS_INFO(ns.c_str());

    std::thread loop_closure_thread(&distributedMapping::loopClosureThread, &dm);
    std::thread visualize_map_thread(&distributedMapping::globalMapThread, &dm);
    // 订阅激光雷达点云话题
    ros::Subscriber laserCloud_QU = nh.subscribe<sensor_msgs::PointCloud2>(name_ + "/scan_undistorted", 200000, standard_pcl_vel);
    // 订阅里程计话题
    ros::Subscriber odom_QU = nh.subscribe<nav_msgs::Odometry>(name_ + "/odom", 200000, odom_callback);
    // 发布高清地图话题
    ros::Publisher pub_map_high = nh.advertise<sensor_msgs::PointCloud2>(name + "/map_high", 1);
    // 发布修正后的odom

    // 初始化点云容器和里程计容器
    pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudIn_temp(new pcl::PointCloud<pcl::PointXYZI>());
    nav_msgs::Odometry::ConstPtr odomIn(new nav_msgs::Odometry());
    ROS_INFO("启动循环");
    ros::Rate rate(15);
    while (ros::ok())
    {
        ros::spinOnce();

        // 从lidar_buffer_front和odom_buffer_front中取出数据
        // if (lidar_buffer_front.size() <= 20 && odom_buffer_front.size() <= 20)
        if (lidar_buffer_front.size() <= 1 || odom_buffer_front.size() <= 1)
        {
            // ROS_ERROR("lidar_buffer_front or odom_buffer_front is empty, please check the data!");
            continue;
        }
        mtx_buffer_front.lock();
        if (lidar_buffer_front.size() != odom_buffer_front.size())
        {
          
            while (lidar_buffer_front.size() != odom_buffer_front.size())
            {
                // 从较长的队列首部移除元素，直到两个队列的大小相等
                if (lidar_buffer_front.size() > odom_buffer_front.size())
                {
                    lidar_buffer_front.pop_front();
                }
                else
                {
                    odom_buffer_front.pop_front();
                }
            }

            ROS_INFO("lidar_buffer_front.size = odom_buffer_front.size");
        }


        // 取出数据，并弹出使用过的数据
        laserCloudIn_temp = lidar_buffer_front.front();
        lidar_buffer_front.pop_front();
        odomIn = odom_buffer_front.front();
        odom_buffer_front.pop_front();

        // 检查时间戳和header.seq是否一致

        mtx_buffer_front.unlock();
        // ROS_WARN("scan_count: %i , odom_count: %i", scan_count_front, odom_count_front);
        // cout<<"lidar_buffer_front.size"<<lidar_buffer_front.size()<<endl;
        // cout<<"odom_count_front"<<odom_count_front<<endl;
        // ROS_INFO("laserCloudIn_temp->header.seq: %i ; odomIn->header.seq :%i", laserCloudIn_temp->header.seq, odomIn->header.seq);

        // 转换odomIn数据到gtsam::Pose3
        gtsam::Pose3 pose_to = Pose3(
            Rot3::Quaternion(odomIn->pose.pose.orientation.w, odomIn->pose.pose.orientation.x, odomIn->pose.pose.orientation.y, odomIn->pose.pose.orientation.z),
            Point3(odomIn->pose.pose.position.x, odomIn->pose.pose.position.y, odomIn->pose.pose.position.z));
        lidar_end_time = laserCloudIn_temp->header.stamp;
        // ROS_INFO("进入dcl优化");
        if (true)
        // if (dm.saveFrame(pose_to) == true)
        {
            dm.performDistributedMapping(pose_to, laserCloudIn_temp, ros::Time().fromNSec(laserCloudIn_temp->header.stamp * 1000));
            // ROS_INFO("dm.performDistributedMapping 完成");
            dm.updatePoses();
            dm.makeDescriptors();
            // ROS_INFO("dm.makeDescriptors 完成");
            dm.publishPath();
            // ROS_INFO("dm.publishPath 完成");
        }
        // dm.publishTransformation(ros::Time().fromSec(lidar_end_time));
        // ROS_INFO("dm.publishTransformation 完成");
        // 将优化后的位姿存储起来

        if (flg_pub_mapHigh)
        {
            Pose3 latestEstimate = dm.getLatestEstimate();
            float transformTobeMapped[6] = {0};
            transformTobeMapped[0] = latestEstimate.rotation().roll();
            transformTobeMapped[1] = latestEstimate.rotation().pitch();
            transformTobeMapped[2] = latestEstimate.rotation().yaw();
            transformTobeMapped[3] = latestEstimate.translation().x();
            transformTobeMapped[4] = latestEstimate.translation().y();
            transformTobeMapped[5] = latestEstimate.translation().z();

            pcl::PointCloud<pcl::PointXYZI>::Ptr cloudOut(new pcl::PointCloud<pcl::PointXYZI>());
            PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
            *cloudOut = *transformPointCloud(laserCloudIn_temp, &thisPose6D);
             sensor_msgs::PointCloud2 tempCloud;
            pcl::toROSMsg(*cloudOut, tempCloud);
            tempCloud.header.stamp = ros::Time().fromNSec(laserCloudIn_temp->header.stamp * 1000);
            tempCloud.header.frame_id = "world";
            if (pub_map_high.getNumSubscribers() != 0)
                pub_map_high.publish(tempCloud);

            // publishCloud(pub_map_high, cloudOut, laserCloudIn_temp->header.stamp, name_ + "/" + laserCloudIn_temp->header.frame_id);
        }

        // 清空laserCloudIn_temp和odomIn
        laserCloudIn_temp->clear();
        odomIn.reset(new nav_msgs::Odometry());
        // transformTobeMapped[6] = {0};
        // rate.sleep();
    }

    loop_closure_thread.join();
    visualize_map_thread.join();

    return 0;
}
